/**************************************************************
*
* M3D_TRS.CPP
*
* Transformations based on quaternions support
**************************************************************/

#include <m3d/m3d_stdafx.h>

#include <ap_comm.h>
#include <m3d.h>

m3dTR m3dTrIdentity(m3dVZero, m3dQUAT());

/**************************************************************
* m3dTR::Identity()
*
**************************************************************/
void m3dTR::Identity (void)
{
   tran = m3dVZero;
   rot.Identity();
}

/**************************************************************
* m3dTR::Invert()
*
**************************************************************/
void m3dTR::Invert (void)
{
   rot.Invert();
   rot.Transform(&tran);
   m3dNegateVector(&tran);
}

/**************************************************************
* m3dTR::Inverse()
*
**************************************************************/
void m3dTR::Invert (m3dTR *res) const
{
   *res = *this;
   res->Invert();
}

/**************************************************************
* m3dTR::SetTransform()
*
**************************************************************/
void m3dTR::SetTransform (const m3dV* transVec, const m3dQUAT* rotateQuat)
{
   SetRotation(rotateQuat);
   SetTranslation(transVec);
}

/**************************************************************
* m3dTR::SetTranslation()
*
**************************************************************/
void m3dTR::SetTranslation (const m3dV* transVec)
{
   tran.x = transVec->x;
   tran.y = transVec->y;
   tran.z = transVec->z;
}

/**************************************************************
* m3dTR::SetRotation()
*
**************************************************************/
void m3dTR::SetRotation (const m3dQUAT* rotateQuat)
{
   rot.s = rotateQuat->s;
   rot.v = rotateQuat->v;
}

/**************************************************************
* m3dTR::SetMatr()
*
**************************************************************/
void m3dTR::SetMatr (const m3dMATR* matr)
{
   rot.SetMatr(matr);
   //matr->GetTranslate(&tran);
   matr->GetOrigin(&tran);
}

/**************************************************************
* m3dTR::GetMatr()
*
**************************************************************/
void m3dTR::GetMatr (m3dMATR *matr) const
{
   rot.GetMatr(matr);
   matr->Translate(&tran, M3D_POST);
}

/**************************************************************
* m3dTR::Transform()
*
**************************************************************/
void m3dTR::Transform (m3dV *vct) const
{
   rot.Transform(vct);
   vct->x += tran.x;
   vct->y += tran.y;
   vct->z += tran.z;
}

/**************************************************************
* m3dTR::Transform()
*
**************************************************************/
void m3dTR::Transform (const m3dV *a, m3dV *res) const
{
   rot.Transform(a, res);
   res->x += tran.x;
   res->y += tran.y;
   res->z += tran.z;
}

/***************************************************************
* m3dTR::Transform ()
*
***************************************************************/
void m3dTR::Transform (const m3dTR *tr, m3dTR *res) const
{
   if (m3dIsZeroTr_A(this, M3D_EPSILON)) {
      *res = *tr;
      return;
   }
   m3dTR qInv, qTemp;
   Invert(&qInv);

   m3dMultiplyTr(this,   tr,   &qTemp);
   m3dMultiplyTr(&qTemp, &qInv, res);
}

/***************************************************************
* m3dTR::Transform ()
*
***************************************************************/
void m3dTR::Transform (m3dTR *tr) const
{
   m3dTR tmp = *tr;
   Transform(&tmp, tr);
}

/***************************************************************
* m3dTR::Transform ()
*
***************************************************************/
void m3dTR::Transform (m3dMATR *matr, int OperOrder) const
{
   rot.Transform(matr, OperOrder);
   matr->Translate(&tran, OperOrder);
}

/***************************************************************
* m3dTR::ApplyTo ()
*
***************************************************************/
void m3dTR::ApplyTo (m3dMATR *matr) const
{
   matr->Translate(&tran, M3D_POST);
   rot.Transform(matr, M3D_PRE);
}

/***************************************************************
* m3dTR::SetDiff ()
*
***************************************************************/
void m3dTR::SetDif (const m3dTR *trFrom, const m3dTR *trTo)
{
   m3dQUAT qFromInv;
   trFrom->rot.Invert(&qFromInv);
   m3dMultiplyQuat(&trTo->rot, &qFromInv, &rot);
   m3dSubtractVector(&trTo->tran, &trFrom->tran, &tran);
}

/**************************************************************
* m3dTR::CheckValid()
*
**************************************************************/
BOOL m3dTR::CheckValid(void) const
{
#ifndef _RETAIL
   if (!_m3dCheckValid(&tran)) {
      return FALSE;
   }

   if (!m3dCheckFinite(rot.s)) {
      return FALSE;
   }

   if (!_m3dCheckValid(&rot.v)) {
      return FALSE;
   }
#endif
   return TRUE;
}
/**************************************************************
* m3dIsEqualTransform_A()
*
**************************************************************/
BOOL m3dIsEqualTr_A (const m3dTR *a, const m3dTR *b, float acc)
{
   if (!m3dIsEqualPoint_A(&a->tran, &b->tran, acc)) {
      return FALSE;
   }
   if (!m3dIsEqualQuat_A(&a->rot, &b->rot, acc)) {
      return FALSE;
   }
   return TRUE;
}

/***************************************************************
* m3dIsZeroTr_A ()
*
***************************************************************/
BOOL m3dIsZeroTr_A (const m3dTR *a, float acc)
{
   if (!m3dIsZeroQuat_A(&a->rot, acc)) {
      return FALSE;
   }
   if (!m3dIsEqualPoint_A(&a->tran, &m3dVZero, acc)) {
      return FALSE;
   }

   return TRUE;
}

//
// End of file 'M3D_TR.CPP'
//
